#include <rtthread.h>

#define THREAD_PRIORITY 25
#define THREAD_TIMESLICE 5


#define BENCHMARKS 50000

static volatile uint64_t dirOverhead = 0;
static volatile uint64_t loopCycle = 0;

static volatile uint32_t count1;
static volatile uint32_t count2;

static char thread2_stack[4096];
static struct rt_thread thread2;

static volatile uint64_t g_cycle = 0;

void benchmark_timer_initialize(void)
{
    __asm__ __volatile__ ("MRS %0, CNTPCT_EL0" : "=r"(g_cycle) :: "memory", "cc");

}
uint64_t benchmark_timer_read(void)
{
    uint64_t end;
    __asm__ __volatile__ ("MRS %0, CNTPCT_EL0" : "=r"(end) :: "memory", "cc");
    return end - g_cycle;
}

uint64_t GetGenericTimerFreq(void)
{
    uint64_t freq;

     __asm__ __volatile__ ("MRS %0, CNTFRQ_EL0" : "=r"(freq) : : "memory", "cc");

    return freq;
}

#define put_time( _message, _total_time, \
                  _iterations, _loop_overhead, _overhead ) \
    rt_kprintf( \
      "%s - %d\n", \
      (_message), \
      (((_total_time) - (_loop_overhead)) / (_iterations)) \
    )

static void thread2_entry(void *param)
{
    uint64_t telapsed;
    benchmark_timer_initialize();

    for (count1 = 0; count1 < BENCHMARKS - 1; count1++) {
	    rt_thread_yield();
    }

    telapsed = benchmark_timer_read();
    rt_kprintf("Context switch latency:telapsed %u\n",telapsed);

    put_time(
        "Rhealstone: Task switch",  telapsed, (BENCHMARKS * 2) - 1,
        loopCycle,
        dirOverhead);
  
}

static char thread1_stack[4096];
static struct rt_thread thread1;
static void thread1_entry(void *param)
{
    rt_thread_startup(&thread2);
    rt_thread_yield();

    for (count2 = 0; count2 < BENCHMARKS; count2++) {
	rt_thread_yield();
    }

}


int main()
{
    rt_thread_mdelay(2000);

    uint64_t freq = GetGenericTimerFreq();
    rt_kprintf("Context switch latency:Freq %u\n",freq);
  
    benchmark_timer_initialize();
    for (count1 = 0; count1 < BENCHMARKS - 1; count1++) {
        __asm__ __volatile__("");
    }
    for (count2 = 0; count2 < BENCHMARKS; count2++) {
        __asm__ __volatile__("");
    }
    loopCycle = benchmark_timer_read();

    rt_kprintf("Context switch latency:loopCycle %u\n",loopCycle);

    dirOverhead = 0;

    rt_enter_critical();
    for(int i = 0; i < BENCHMARKS; i++) {
	benchmark_timer_initialize();
	rt_thread_yield();
	dirOverhead += benchmark_timer_read();
    }
    rt_exit_critical();
    dirOverhead /= BENCHMARKS;

    rt_kprintf("Context switch latency:dirOverhead %u\n",dirOverhead);

  rt_thread_init(&thread1,
            "thread1",
            thread1_entry,
            RT_NULL,
            &thread1_stack[0],
            sizeof(thread1_stack),
            THREAD_PRIORITY - 1, THREAD_TIMESLICE);


  rt_thread_init(&thread2,
            "thread2",
            thread2_entry,
            RT_NULL,
            &thread2_stack[0],
            sizeof(thread2_stack),
            THREAD_PRIORITY - 1, THREAD_TIMESLICE);

  rt_thread_startup(&thread1);

  return 0;

}
